function q = ikine(p)

x = p(1); y = p(2); z = p(3);

l1 = 1; l2 = 1;
l0 = sqrt(x^2 + z^2);
q0 = atan2(z/l0, x/l0);

q1 = acos((x^2 + z^2 + l2^2 - l1^2) / (2 * l1 * l0)) + q0;
q2 = -acos((x^2 + z^2 + l1^2 - l2^2) / (2 * l2 * l0)) + q0 - q1;

q = [q1; q2];